
#include "asm.h"

.equ    KERNEL, 0
.equ    USER,   312
.equ    NESTED_COUNT, 624

 
LEAF(asm_start)
    mtc0    zero, CP0_STATUS
    mtc0    zero, CP0_WATCHLO
    mtc0    zero, CP0_WATCHHI

    mfc0    t0, CP0_CONFIG
    and     t0, ~0x7
    ori     t0, 0x2
    mtc0    t0, CP0_CONFIG
    
    jr  ra
END(asm_start)

.macro SAVE_CONTEXT offset
    .set    noat
    ld    k0, current_running
    addi  k0, k0, \offset
    
    sd    zero,  OFFSET_REG0(k0)
    sd    AT,    OFFSET_REG1(k0) 
    sd    v0,    OFFSET_REG2(k0)
    sd    v1,    OFFSET_REG3(k0)
    sd    a0,    OFFSET_REG4(k0)
    sd    a1,    OFFSET_REG5(k0)
    sd    a2,    OFFSET_REG6(k0)
    sd    a3,    OFFSET_REG7(k0)
    sd    t0,    OFFSET_REG8(k0)
    sd    t1,    OFFSET_REG9(k0)
    sd    t2,    OFFSET_REG10(k0)
    sd    t3,    OFFSET_REG11(k0)
    sd    t4,    OFFSET_REG12(k0)
    sd    t5,    OFFSET_REG13(k0)
    sd    t6,    OFFSET_REG14(k0)
    sd    t7,    OFFSET_REG15(k0)
    sd    s0,    OFFSET_REG16(k0)
    sd    s1,    OFFSET_REG17(k0)
    sd    s2,    OFFSET_REG18(k0)
    sd    s3,    OFFSET_REG19(k0)
    sd    s4,    OFFSET_REG20(k0)
    sd    s5,    OFFSET_REG21(k0)
    sd    s6,    OFFSET_REG22(k0)
    sd    s7,    OFFSET_REG23(k0)
    sd    t8,    OFFSET_REG24(k0)
    sd    t9,    OFFSET_REG25(k0)
    
    # k0 and k1 can't store
    
    sd    gp,    OFFSET_REG28(k0)
    sd    sp,    OFFSET_REG29(k0)
    sd    fp,    OFFSET_REG30(k0)
    sd    ra,    OFFSET_REG31(k0)
    
    mfc0  k1,    CP0_STATUS
    sd    k1,    OFFSET_STATUS(k0)
    
    mfc0  k1,    CP0_CAUSE
    sd    k1,    OFFSET_CAUSE(k0)
    
    mfc0  k1,    CP0_TAGHI
    sd    k1,    OFFSET_HI(k0)
    
    mfc0  k1,    CP0_TAGLO
    sd    k1,    OFFSET_LO(k0)
    
    mfc0  k1,    CP0_BADVADDR
    sd    k1,    OFFSET_BADVADDR(k0)
    
    mfc0  k1,    CP0_EPC
    sd    k1,    OFFSET_EPC(k0)
    
    
    .set    at
.endm

.macro RESTORE_CONTEXT offset
    .set    noat
    ld    k0, current_running
    addi  k0, k0, \offset
    
    ld    zero,  OFFSET_REG0(k0)
    ld    AT,    OFFSET_REG1(k0) 
    ld    v0,    OFFSET_REG2(k0)
    ld    v1,    OFFSET_REG3(k0)
    ld    a0,    OFFSET_REG4(k0)
    ld    a1,    OFFSET_REG5(k0)
    ld    a2,    OFFSET_REG6(k0)
    ld    a3,    OFFSET_REG7(k0)
    ld    t0,    OFFSET_REG8(k0)
    ld    t1,    OFFSET_REG9(k0)
    ld    t2,    OFFSET_REG10(k0)
    ld    t3,    OFFSET_REG11(k0)
    ld    t4,    OFFSET_REG12(k0)
    ld    t5,    OFFSET_REG13(k0)
    ld    t6,    OFFSET_REG14(k0)
    ld    t7,    OFFSET_REG15(k0)
    ld    s0,    OFFSET_REG16(k0)
    ld    s1,    OFFSET_REG17(k0)
    ld    s2,    OFFSET_REG18(k0)
    ld    s3,    OFFSET_REG19(k0)
    ld    s4,    OFFSET_REG20(k0)
    ld    s5,    OFFSET_REG21(k0)
    ld    s6,    OFFSET_REG22(k0)
    ld    s7,    OFFSET_REG23(k0)
    ld    t8,    OFFSET_REG24(k0)
    ld    t9,    OFFSET_REG25(k0)
    
    # k0 and k1 can't store
    
    ld    gp,    OFFSET_REG28(k0)
    ld    sp,    OFFSET_REG29(k0)
    ld    fp,    OFFSET_REG30(k0)
    ld    ra,    OFFSET_REG31(k0)
    
    ld    k1,    OFFSET_STATUS(k0)
    mtc0  k1,    CP0_STATUS
    
    ld    k1,    OFFSET_CAUSE(k0)
    mtc0  k1,    CP0_CAUSE
    
    ld    k1,    OFFSET_HI(k0)
    mfc0  k1,    CP0_TAGHI
    
    ld    k1,    OFFSET_LO(k0)
    mtc0  k1,    CP0_TAGLO
    
    ld    k1,    OFFSET_BADVADDR(k0)
    mtc0  k1,    CP0_BADVADDR
    
    ld    k1,    OFFSET_EPC(k0)
    mtc0  k1,    CP0_EPC
    
   .set    at
.endm

# function do_scheduler
NESTED(do_scheduler, 0, ra)
    SAVE_CONTEXT(KERNEL)          # save current running task context
    jal  scheduler                # jump to scheduler and run another task
    RESTORE_CONTEXT(KERNEL)       # restore context into register
    jr   ra
END(do_scheduler)

LEAF(reset_timer)
    li     a0, 15000000
    mtc0   zero, CP0_COUNT
    nop
    mtc0   a0, CP0_COMPARE
    nop
    jr     ra
END(reset_timer)

LEAF(set_cp0_status)
    mtc0    a0, CP0_STATUS
    jr      ra
END(set_cp0_status)

LEAF(set_cp0_cause)
   
END(set_cp0_cause)

LEAF(get_cp0_status)
    mfc0    v0, CP0_STATUS
    jr      ra
END(get_cp0_status)
    
LEAF(get_cp0_cause)
    mfc0    v0, CP0_CAUSE
    jr      ra
END(get_cp0_cause)

LEAF(get_cp0_count)
	  mfc0    v0, CP0_COUNT
    jr      ra
END(get_cp0_count)

LEAF(get_cp0_compare)
	  mfc0    v0, CP0_COMPARE
    jr      ra
END(get_cp0_compare)

LEAF(get_cp0_epc)
    mfc0    v0, CP0_EPC
    jr      ra
END(get_cp0_epc)
 
.global exception_handler_begin
.global exception_handler_end
.global TLBexception_handler_begin
.global TLBexception_handler_end

# close interrupt
.macro CLI
    mfc0    k0, CP0_STATUS
    li      k1, 0xfffffffe
    and     k0, k0, k1            # put 0 on the end of CP0_STATUS
    mtc0    k0, CP0_STATUS
.endm

# start interrupt
.macro STI
    mfc0    k0, CP0_STATUS
    li      k1, 0x1
    or      k0, k0, k1
    mtc0    k0, CP0_STATUS
.endm

NESTED(TLBexception_handler_entry, 0, sp)
TLBexception_handler_begin:
	 
 
TLBexception_handler_end:
END(TLBexception_handler_entry)

NESTED(exception_handler_entry, 0, sp)   
exception_handler_begin:
    SAVE_CONTEXT(USER)
    
    # change the state of kernel_state
    ld      t0, current_running
    li      t1, 1
    sw      t1, OFFSET_KERNEL_STATE(t0)
    
    mfc0    k0, CP0_CAUSE
    andi    k0, k0, CAUSE_EXCCODE    # CAUSE_EXCCODE = 01111100
    sll     k0, k0, 1                # k0 = exccode * 8
    
    dla     k1, exception_handlers   # move exception_handler base to k1
    add     k1, k0, k1               # entry_addr = exccode * 8 + base
    ld      k1, 0(k1)                # get the addr of different handle_exception
    jr      k1                       # jump to entry_addr
exception_handler_end:
END(exception_handler_entry)

NESTED(handle_int, 0, sp)
    mfc0   a0, CP0_STATUS
    mfc0   a1, CP0_CAUSE

    addiu  sp, sp, -16
    jal    interrupt_helper
    addiu  sp, sp, 16
    
    SAVE_CONTEXT(KERNEL)  
    j     exception_handler_exit
END(handle_int)

NESTED(handle_syscall, 0, sp)
    add    a3, a2, $0
    add    a2, a1, $0
    add    a1, a0, $0
    add    a0, v0, $0
    
    jal    system_call_helper
    
    SAVE_CONTEXT(KERNEL)
    j      exception_handler_exit
END(handle_syscall)

NESTED(handle_tlb, 0, sp)  
    
END(handle_tlb)

NESTED(handle_other, 0, sp)
    jal    other_exception_handler
    
    SAVE_CONTEXT(KERNEL)
    j      exception_handler_exit
   
END(handle_other)

LEAF(exception_handler_exit)
    # change the state of kernel_state
    ld      t0, current_running
    li      t1, 0
    sw      t1, OFFSET_KERNEL_STATE(t0)
    RESTORE_CONTEXT(USER)
    eret
END(exception_handler_exit)

# ADD entry
LEAF(first_entry)
    jal     reset_timer
    j       exception_handler_exit
END(first_entry)

LEAF(set_cp0_entryhi)
   
END(set_cp0_entryhi)


LEAF(get_cp0_index)
    
END(get_cp0_index)

LEAF(set_cp0_index)
    
END(set_cp0_index)

LEAF(get_cp0_badvaddr)
    mfc0    v0, CP0_BADVADDR
    jr      ra
END(get_cp0_badvaddr)

LEAF(get_cp0_entrylo0)
    
END(get_cp0_entrylo0)

LEAF(set_cp0_entrylo0)
    
END(set_cp0_entrylo0)

LEAF(get_cp0_entrylo1)
    
END(get_cp0_entrylo1)

LEAF(set_cp0_entrylo1)
   
END(set_cp0_entrylo1)

LEAF(set_cp0_pagemask)
    
END(set_cp0_pagemask)

LEAF(tlbwr_operation)
   
END(tlbwr_operation)

LEAF(tlbwi_operation)
    
END(tlbwi_operation)

LEAF(tlbp_operation)
    
END(tlbp_operation)


LEAF(get_cp0_config)
    
END(get_cp0_config)

LEAF(get_cpu_id)
    
END(get_cpu_id)
